// Created by Chengfu Zou
// Maintained by Chengfu Zou, Labor
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef ARMOR_SOLVER_SOLVER_HPP_
#define ARMOR_SOLVER_SOLVER_HPP_

// std
#include <memory>
// ros2
#include <tf2_ros/buffer.h>

#include <rclcpp/time.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
// 3rd party
#include <Eigen/Dense>
// project
#include "rm_interfaces/msg/gimbal_cmd.hpp"
#include "rm_interfaces/msg/target.hpp"
#include "rm_utils/math/trajectory_compensator.hpp"

namespace fyt::auto_aim {

class newRollingArray{
public:
  newRollingArray():_size(100),id(0){metric.setZero();}

  newRollingArray(size_t _size):_size(100),id(0){metric.setZero();}

  void update(Eigen::Vector3d elem)
  {
    if(array.size()<_size)
    {
      metric +=elem;
      array.push_back(elem);
      return;
    }
    metric -=array[id];
    array[id] = elem;
    metric +=array[id];
    id = ((id+1)%_size+_size)%_size;
  }

  Eigen::Vector3d getMetric(){return metric;}

  int size(){return array.size();}

  int getMaxSize(){return _size;}

  void clear(){
    array.clear();
     metric.setZero();
  }

  Eigen::Vector3d getMeanOfNearestPoints()
  {
    Eigen::Vector3d meanPoint(0, 0, 0);
    for(size_t i = 0;i<array.size();i++)
    {
      meanPoint += array[i];
    }
    meanPoint /= static_cast<double>(array.size());
    return meanPoint;
  }

private:
  size_t _size;
  int id;
  std::vector<Eigen::Vector3d> array;
  Eigen::Matrix<double,3,1> metric;

};


// Solver class used to solve the gimbal command from tracked target
class Solver {
public:
  explicit Solver(std::weak_ptr<rclcpp::Node> node);
  // explicit Solver(std::string trajectory_compensator_type, float max_tracking_v_yaw);
  ~Solver() = default;

  // Solve the gimbal command from tracked target
  // Throw: tf2::TransformException if the transform from "odom_vision" to "gimbal_link" is not available
  rm_interfaces::msg::GimbalCmd solve(const rm_interfaces::msg::Target &target_msg,
                                      const rclcpp::Time &current_time,
                                      std::shared_ptr<tf2_ros::Buffer> tf2_buffer_,
                                     double &want_yaw,
                                    double &want_pitch,
                                    double &min_limit_angle,
                                    double &max_limit_angle,
                                    int &index,std::vector<Eigen::Vector3d> &fire_target,
                                    std::vector<Eigen::Vector3d> &shoot_target);

  enum State { TRACKING_ARMOR = 0, TRACKING_CENTER = 1 } state;

  std::vector<std::pair<double, double>> getTrajectory(double distance,
                                                       double angle) const noexcept {
    return trajectory_compensator_->getTrajectory(distance, angle);
  }

  bool is_outpost;
  newRollingArray all_target_center;//前哨战
  newRollingArray target_center;//非前哨战
  double normal_v;

private:
  // Get the armor positions from the target robot
  std::vector<Eigen::Vector3d> getArmorPositions(const Eigen::Vector3d &target_center,
                                                 const double yaw,
                                                 const double r1,
                                                 const double r2,
                                                 const double dz,
                                                 const size_t armors_num) const noexcept;

  // Select the best armor to shoot
  // Return: selected idx in {0, 1, ..., armors_num - 1}
  int selectBestArmor(const std::vector<Eigen::Vector3d> &armor_positions,
                      const Eigen::Vector3d &target_center,
                      const double target_yaw,
                      const double target_v_yaw,
                      const size_t armors_num) const noexcept;

  void calcYawAndPitch(const Eigen::Vector3d &p,
                       const std::array<double, 3> rpy,
                       double &yaw,
                       double &pitch,const double &v_yaw) const noexcept;

  bool isOnTarget(const double cur_yaw,
                  const double cur_pitch,
                  const double target_yaw,
                  const double target_pitch,
                  const double distance,
                  const Eigen::Vector3d target_position,
                  const double v_yaw,
                  const double ekf_yaw)  noexcept;

  int selectHighSpeedArmor(const std::vector<Eigen::Vector3d>& poses,
                                 const Eigen::Vector3d& center,
                                 double raw_theta,      // 目标相对角度（rad）
                                 double raw_omega,      // 目标角速度（rad/s）
                                 size_t armors_num,
                                 double dt) noexcept;
                                 
    double calculateYawLimit(double enemy_v_yaw, double max_v_yaw, double min_v_yaw, 
                         double max_angle, double min_angle)noexcept;

      double adjustPitchWithSpeed(double pitch, double v_yaw, double k = 0.008, double base_speed = 1.5, double max_compensate = 0.05)noexcept;

  std::unique_ptr<TrajectoryCompensator> trajectory_compensator_;

  double prediction_delay_;
  double controller_delay_;

  double shooting_range_w_;
  double shooting_range_h_;

  double max_tracking_v_yaw_;
  int overflow_count_;
  int transfer_thresh_;

  double side_angle_;
  double min_switching_v_yaw_;

  int    last_idx_;           // 上次选板索引
  int    hold_count_;         // 连续保持计数
  const int    hold_thresh_  = 3;    // 连续 N 帧后允许切换
  const double omega_thresh_ = 4.0;  // (rad/s) 高速阈值
  double last_omega_;     

  double no_fire;   


  double max_v_yaw; // 最大敌方转速
  double min_v_yaw;  // 最小敌方转速


  bool last_fire;





  std::weak_ptr<rclcpp::Node> node_;
};
}  // namespace fyt::auto_aim
#endif  // ARMOR_SOLVER_SOLVER_HPP_
